﻿#coding: utf-8

from math import sqrt

xi = [2,5,9]
yi = [2,5,9]
zi = [2,5,9]
ri = [5,7,1]

FATOR = 1000.0
FATOR2= 10

n = 3
def soma_v(v1,v2):
	res = [0]*len(v1)
	for i in range(0,len(v1)):
		res[i] = v1[i]+v2[i]
	return res
	
def interno_v(v1,v2):
	res = 0
	for i in range(0,len(v1)):
		res += v1[i]*v2[i]
		
	return res	
	
def escalar_v(v1,k):
	res = [0]*len(v1)
	for i in range(0,len(v1)):
		res[i] = v1[i]*k
		
	return res	
	
def norma(v):
	res = 0
	for i in range(0,len(v)):
		res+=pow(v[i],2)
	res = sqrt(res)
	return res
	

def Ei(i,v,x,y,z,r):
	ind=3*i
	res=pow( (x-v[ind]),2 ) + pow( (y-v[ind+1]),2 ) + pow( (z-v[ind+2]),2 ) - pow( (r+ri[i]),2 )
	
	return res/FATOR
	
def der_Ei_k(i,k,v,x,y,z,r):
	ind_i=3*i
	ind_k=3*k
	
	grad = [0]*3
	
	grad[0] = 2*(v[ind_k]-v[ind_i])/FATOR
	grad[1] = 2*(v[ind_k+1]-v[ind_i+1])/FATOR
	grad[2] = 2*(v[ind_k+2]-v[ind_i+2])/FATOR
	
	return grad
	
def Fi(i,v):
	ind=3*i
	res = pow(v[ind],2)+pow(v[ind+1],2)+pow(v[ind+2],2)
	for j in range(0,n):
		if j!=i:
			res+= FATOR2*pow(Ei(j,v,v[ind],v[ind+1],v[ind+2],ri[i]),2)
			
	return res/FATOR
	
def F(v):
	res = 0
	for i in range(0,n):
		res+=Fi(i,v)
		
	return res
	
def der_Fi_k(i,k,v):
	ind=3*i
	grad = [0]*3
	
	if i==k:
		grad[0] = 2*v[ind]
		grad[1] = 2*v[ind+1]
		grad[2] = 2*v[ind+2]
		for j in range(0,n):
			if j!=i:
				ei = Ei(j,v,v[ind],v[ind+1],v[ind+2],ri[i])
				dek = der_Ei_k(j,i,v,v[ind],v[ind+1],v[ind+2],ri[i])
				
				grad[0]+= 2*ei + FATOR2*dek[0]
				grad[1]+= 2*ei + FATOR2*dek[1]
				grad[2]+= 2*ei + FATOR2*dek[2]
	else:
		ei = Ei(k,v,v[ind],v[ind+1],v[ind+2],ri[i])		
		dfk = escalar_v( der_Ei_k(k,i,v,v[ind],v[ind+1],v[ind+2],ri[i]),-2 )
		
		grad[0] = FATOR2*(2*ei + dfk[0])
		grad[1] = FATOR2*(2*ei + dfk[1])
		grad[2] = FATOR2*(2*ei + dfk[2])
		
	return grad
	
def der_F_k(k,v):
	grad=[0]*3
	for i in range(0,n):
		dfik = der_Fi_k(i,k,v)
		grad[0] += dfik[0]
		grad[1] += dfik[1]
		grad[2] += dfik[2]
		
	return grad
				
		
def grad_F(v):
	grad=[0]*3*n
	for k in range(0,n):
		ind=3*k
		dfk = der_F_k(k,v)
		
		grad[ind] = dfk[0]
		grad[ind+1] = dfk[1]
		grad[ind+2] = dfk[2]
		
	return grad
	
	
def v_from_vec():
	v = [0]*3*n
	for i in range(0,n):
		ind = 3*i
		v[ind] = xi[i]
		v[ind+1] = yi[i]
		v[ind+2] = zi[i]
		
	return v	
	
def busca_linear(xk,dk):
	sigma = 0.8
	lbda = 1.0
	lim = 10	
	
	while( F( soma_v(xk,escalar_v(dk,lbda)) ) >= F(xk) + sigma*lbda*interno_v(grad_F(xk),dk) ):
		if lim == 0:
			break			
		lbda/=2
		lim-=1
	
	return lbda
		
def main():
	x0 = v_from_vec()
	dk=[1]*len(x0)
	epson = 1e-2
	it = 0
	
	while(norma(dk)>epson):
		if it==4000:
			break
			
		dk = escalar_v(grad_F(x0),-1)
		lbda = busca_linear(x0,dk)
		x0 = soma_v(x0,escalar_v(dk,lbda))
		
	
		it+=1
	
	for i in range(0,n):
		ind = 3*i
		print "P",i,"= ( ",x0[ind],x0[ind+1],x0[ind+2],ri[i]," )"
	
	print it
	return x0
	
x0=main()
